#include "stm32f10x.h"                  // Device header
#include "pwm.h"
#include "Delay.h"

void Servo_Init(void)
{
	PWM_Init();
}
/*
PWM_SetCompare2(500);	//舵机0°
PWM_SetCompare2(1500);	//舵机90°
PWM_SetCompare2(2500);	//舵机180°
*/
void Servo0_SetAngle(float Angle)		//	Servo_base 舵机底座 控制左右
{
	PWM_SetCompare1(Angle / 180 * 2000 + 500);
}

void Servo1_SetAngle(float Angle)		//	Servo_foreArm 舵机后臂 控制前后
{
	PWM_SetCompare2(Angle / 180 * 2000 + 500);
}

void Servo2_SetAngle(float Angle)		//	Servo_RearArm 舵机前臂 控制上下
{
	PWM_SetCompare3(Angle / 180 * 2000 + 500);
}

void Servo3_SetAngle(float Angle)		//	Servo_gripper 舵机抓手 末端执行器
{
	PWM_SetCompare4(Angle / 180 * 2000 + 500);
}

void Servo_SetAngle(float A1,float A2,float A3,float A4)		//	Servo
{
	PWM_SetCompare1(A1 / 180 * 2000 + 500);
	Delay_ms(600);
	PWM_SetCompare2(A2 / 180 * 2000 + 500);
	Delay_ms(500);
	PWM_SetCompare3(A3 / 180 * 2000 + 500);
	Delay_ms(600);
	PWM_SetCompare4(A4 / 180 * 2000 + 500);
//	Delay_s(1);
}

